#include "stanley.h"

Stanley::Stanley(){
    kpLinear_ = 0.5;
}

Stanley::~Stanley(){

}


// 计算局部目标点的速度
// mc::status Stanley::computeTwist(geometry_msgs::PoseStamped goal, geometry_msgs::Twist curr_vel, geometry_msgs::Twist& cmd_vel){
mc::status Stanley::computeTwist(mc::TrajectoryPoint2D goal, geometry_msgs::Twist curr_vel,
    geometry_msgs::Twist& cmd_vel){

    mc::status status;
    
    // // 转换为相对机器人位姿的目标点
    // geometry_msgs::PoseStamped local_goal = convertGlobalPoseToLocal(goal, robot_posestamp_);
    // double goal_x = local_goal.pose.position.x;
    // double goal_y = local_goal.pose.position.y;

    // // 前瞻距离
    // double lookhead_distance = hypot(goal_x, goal_y);
    // // 横向误差（假设机器人本地坐标的路径点）
    // double lateral_error = goal_y;
    // // 航向误差（机器人朝向与目标点方向的角度差）
    // double heading_error = utils::math::diffAngle(0.0, atan2(goal_y, goal_x));
    // // 转向角
    // double steering_angle = heading_error + std::atan2(lateral_error, lookhead_distance);
    // cout << "[Stanley] steering_angle: " << steering_angle 
    //     << ", lateral_error: " << lateral_error << endl;

    // if(isPositionReached(robot_posestamp_, local_goal)){
    //     if(!isAngleReached(robot_posestamp_, local_goal)){
    //         rotateToTGoal(robot_posestamp_, local_goal, cmd_vel);
    //     }
    // } 
    // else{
    //     // 距离越远，速度越快
    //     cmd_vel.linear.x = kpLinear_ * lookhead_distance;  // 简单的PID控制
    //     cmd_vel.linear.y = 0.0;
    //     cmd_vel.angular.z = cmd_vel.linear.x * tan(steering_angle);
    // }    

    return status;
}